/*
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 * File: arm_fk.c
 *
 * MATLAB Coder version            : 5.5
 * C/C++ source code generated on  : 14-Dec-2023 15:35:26
 */

/* Include Files */
#include "arm_fk.h"
#include <math.h>

/* Function Definitions */
/*
 * Arguments    : float theta1
 *                float theta2
 *                float theta3
 *                float theta4
 *                float *X
 *                float *Y
 *                float *Z
 *                float *Rx
 *                float *Ry
 *                float *Rz
 * Return Type  : void
 */
void arm_fk(float theta1, float theta2, float theta3, float theta4, float *X, float *Y, float *Z, float *Rx, float *Ry, float *Rz)
{
	float Lt,theta, bias;
/* 求解偏移量 */
	Lt = L2*sin(theta2)+L3*sin(theta2+theta3)+L4*sin(theta2+theta3+theta4);
	*Z = L2*cos(theta2)+L3*cos(theta2+theta3)+L4*cos(theta2+theta3+theta4)+L1;
	*Y = Lt*sin(theta1);
	*X = Lt*cos(theta1);

/* 求解旋转量(ZYX) */
	theta = theta2+theta3+theta4;
	//printf("theta=%0.6f, theta2=%0.2f, theta3=%0.2f, theta4=%0.2f\r\n",theta,rad2deg(theta2),rad2deg(theta3),rad2deg(theta4));
	bias = fabs(theta)-RAD_90;
	//printf("err:%0.6f\r\n",bias);

	if (bias>0.01f)           						// 角度和与90度的差值大于0
	{
		if(theta>0.0f)
		{
			*Ry = PI-theta;
			if(theta1>=0.0f)
			{
				*Rz = -PI+theta1;
	    	}
	    	else
	     	{
	    		*Rz = PI+theta1;
	    	}
		}
		else
		{
			*Ry = -PI-theta;
			if(theta1>=0.0f)
			{
				*Rz = -PI+theta1;
			}
			else
			{
				*Rz = PI+theta1;
	    	}
	  	}
		*Rx = -PI;
	}
	else if (fabs(bias)<0.01f)       				//角度和与90度的差值等于0
	{
		if(theta>0.0f)
		{
			*Ry = PI-theta;
			*Rx = -theta1;
			*Rz = 0.0f;
		}
	 	else if (theta<0.0f)
		{
	 		*Ry = -PI-theta;
	 		*Rx = theta1;
	 		*Rz = 0.0f;
		}
		else
		{
			*Rx = 0.0f;
	    	*Ry = 0.0f;
	    	*Rz = theta1;
		}
	}
	else                    						// 角度和与90度的差值小于0
	{
		*Rx = 0.0f;
		*Ry = theta;
		*Rz = theta1;
	}
	//XDEBUG("[dbg]: fk:[%0.4f %0.4f %0.4f %0.4f %0.4f %0.4f]\r\n", *X, *Y, *Z, rad2deg(*Rx), rad2deg(*Ry), rad2deg(*Rz));
}

/*
 * File trailer for arm_fk.c
 *
 * [EOF]
 */
